// Object which holds pathfinding grid and is used to search
// for paths from point A to point B
// Nick Kitten

#ifndef PATHFINDER_H
#define PATHFINDER_H

#include "Node.h"
#include "Grid.h"
#include "Utilities.h"
#include <queue>
#include <vector>
#include <list>

using namespace std;

class Pathfinder
{
public:
	enum Algorithm{ AStar, Waypoints, HAStar };
	enum Heuristic{ Manhattan, Euclidean };
	enum LimitMode{ None, Depth, Time };

	Pathfinder();
	Pathfinder( const Grid<char> * occupancyGrid, int sideLength, Algorithm method = AStar,
		Heuristic heuristic = Euclidean, LimitMode limit = Depth, int maxLimit = 15,
		bool smooth = true, float delta = 15, float radius = 15 );
	~Pathfinder();

	void loadWaypoints( const Grid<char> * wayPointMap, const Grid<char> * routingTable, 
		const vector<Triple> * wayPointPositions );
	Position posToGrid( Triple real );
	Triple gridToPos( Position pos );

	list<Triple> navigate( Triple start, Triple goal );
	
	void setAlgorithm( Algorithm method );
	void setHeuristic( Heuristic heur );
	void setLimitMode( LimitMode mode );
	void setSmooth( bool smth );
	void setDelta( float newDelta );
	void setRadius( float newRadius );

	
private:
	const Grid<char> * occupancyGrid;
	Grid<bool> touchedList;
	Grid<Node> mapNodes;

	const Grid<char> * wayPointMap;
	const Grid<char> * routingTable; 
	const vector<Triple> * wayPointPositions;

	Algorithm method;
	Heuristic heuristic;
	LimitMode limitMode;
	// max search time in miliseconds
	int maxLimit;
	bool smooth;
	int sideLength;
	// max distance in horizontal plane between occupancy checks
	// when walking the line-of-sight between two points
	float delta;
	// distance around the line-of-sight path to check for occupancy, for an agent with width
	float radius;

	void runAStar( Triple start, Triple goal, list<Triple> & path, int startTime );
	void runWaypoints( Triple start, Triple goal, list<Triple> & path, int startTime );

	Node * popMin( Node * head );
	inline int getDist( Node & currNode, int i, int j )
	{
		int g;

		// diagonals give g = 14, sides are g = 10 with Manhattan
		switch( heuristic )
		{
		case Manhattan:
			g = currNode.g + ( ( i & j ) >> 31 ? 14 : 10 );
			break;
		case Euclidean:
			g = currNode.g + ( ( i & j ) >> 31 ? 20 : 10 );
			break;
		default:
			break;
		}

		#ifdef DEBUG
			assert( g >= 0 );
		#endif

		return g;
	}

	bool isWalkable( Triple begin, Triple end );
	void smoothPath( list<Triple> & path );

	void clearLists();
	void constructPath( const Node * start, const Node * goal, list<Triple> & path );
	int dist( const Position & a, const Position & b );

};

#endif